#include <iostream>
#include <ilcplex/ilocplex.h>
#include <fstream>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <sys/stat.h>
#include "HGJPathSmooth/pathSmoother.h"

using namespace std;

// 创建目录的函数
bool createDirectory(const std::string& dirPath) {
    struct stat info;
    if (stat(dirPath.c_str(), &info) != 0) {
        if (mkdir(dirPath.c_str(), 0777) != 0) {
            std::cerr << "无法创建目录 " << dirPath << std::endl;
            return false;
        }
    } else if (!(info.st_mode & S_IFDIR)) {
        std::cerr << dirPath << " 不是一个目录" << std::endl;
        return false;
    }
    return true;
}

bool readWaypointsAndParams(const std::string& filePath, HGJ::WayPoints& wayPoints, double& param1, double& param2) {
    std::ifstream file(filePath);
    if (!file.is_open()) {
        std::cerr << "无法打开文件 " << filePath << std::endl;
        return false;
    }

    std::string line;
    bool firstLine = true;
    while (std::getline(file, line)) {
        if (line.empty()) {
            continue; // 跳过空行
        }
        std::istringstream iss(line);
        if (firstLine) {
            // 读取参数
            char comma;
            if (!(iss >> param1 >> comma >> param2) || comma != ',') {
                std::cerr << "文件格式错误: " << line << std::endl;
                return false;
            }
            firstLine = false;
        } else {
            // 读取路径点
            double x, y, z;
            char comma1, comma2;
            if (!(iss >> x >> comma1 >> y >> comma2 >> z) || comma1 != ',' || comma2 != ',') {
                std::cerr << "文件格式错误: " << line << std::endl;
                return false;
            }
            wayPoints.push_back({x, y, z});
        }
    }

    file.close();
    return true;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "points_and_lines");
    ros::NodeHandle n;

    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("/pathpoints_visualization_marker", 10);

    ros::Rate r(20);

    HGJ::pathPlanner planner;
    HGJ::WayPoints wayPoints;
    double param1, param2;

    std::string filePath;
    n.param<std::string>("waypoints_file", filePath, "/home/up/pathpoint_ws/src/pathpoint/cfg/waypoints.txt");

    if (!readWaypointsAndParams(filePath, wayPoints, param1, param2)) {
        return 1;
    }

    bool fileWritten = false;
    bool paramsLogged = false;

    while (ros::ok()) {
        ros::spinOnce();

        auto a = planner.genCurv(wayPoints, param1, param2);

        if (!paramsLogged) {
            // 输出生成的参数以进行调试
            for (const auto &param : a) {
                std::cout << "Generated parameter: " << param << std::endl;
            }
            paramsLogged = true; // 设置标志，确保参数只输出一次
        }

        if (!fileWritten) {
            std::string outputPath = "/home/up/pathpoint_ws/src/pathpoint/curv.csv";
            std::string dirPath = "/home/up/pathpoint_ws/src/pathpoint";

            // 检查并创建目录
            if (!createDirectory(dirPath)) {
                return 1;
            }

            std::ofstream fs(outputPath, std::ios::out); // 打开文件流

            if (!fs.is_open()) {
                std::cerr << "无法打开文件 " << outputPath << std::endl;
                return 1;
            }

            // 写入数据到文件
            for (const auto &point : a) {
                fs << point << std::endl;
            }

            fs.close(); // 关闭文件流

            std::cout << "文件已成功写入：" << outputPath << std::endl;
            fileWritten = true; // 设置标志，确保文件只写入一次
        }

        /// set the style of point and line in Rviz
        visualization_msgs::Marker markers, line_strip, line_list;
        markers.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "my_frame";
        markers.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
        markers.ns = line_strip.ns = line_list.ns = "points_and_lines";
        markers.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
        markers.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

        markers.id = 0;
        line_strip.id = 1;
        line_list.id = 2;

        markers.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        line_list.type = visualization_msgs::Marker::LINE_LIST;

        // POINTS markers use x and y scale for width/height respectively
        markers.scale.x = 0.03;
        markers.scale.y = 0.03;

        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
        line_strip.scale.x = 0.01;
        line_list.scale.x = 0.01;

        // Points are green
        markers.color.g = 1.0f;
        markers.color.a = 1.0;

        // Line strip is blue
        line_strip.color.b = 1.0;
        line_strip.color.a = 1.0;

        // Line list is red
        line_list.color.r = 1.0;
        line_list.color.a = 1.0;

        // Create the vertices for the points and lines
        for (const auto & point: a) {
            geometry_msgs::Point p;
            p.x = point.getX();
            p.y = point.getY();
            p.z = point.getZ();
            line_strip.points.push_back(p);
        }

        for (const auto & markPlot: wayPoints) {
            geometry_msgs::Point pp;
            pp.x = markPlot.getX();
            pp.y = markPlot.getY();
            pp.z = markPlot.getZ();

            markers.points.push_back(pp);
        }

        marker_pub.publish(markers);
        marker_pub.publish(line_strip);

        r.sleep();

        ROS_INFO("----------FINISH THIS LOOP----------");
    }
}
